/**
 * File MCLFilter.java Package robocup.component.worldmodel Project Robocup Date
 * Jun 18, 2005 Web www.dai-labor.de
 * 
 * @author Grzegorz Lehmann
 */
package robocup.component.filters;

import robocup.component.geometry.Vektor;

/**
 * <code>MCLFilter</code> used for calculating the players position based on
 * the flags and lines seen.
 * 
 * this filter is less accurate but more efficient then the other and therefore
 * used only when the ball is far.
 * 
 * @author Grzegorz Lehmann
 */
public class MCLFilter {

  private static final int    MAX_PARTICLES        = 50;

  private final Particle[]          xt                   = new Particle[MCLFilter.MAX_PARTICLES];

  private final Particle[]          xt_1                 = new Particle[MCLFilter.MAX_PARTICLES];

  private final Particle            ut                   = new Particle(new Vektor(),
                                                       .9, 0.);

  private final Particle            ut_1                 = new Particle(new Vektor(),
                                                       .9, 0.);
  
  private final Particle            dummyParticle = new Particle(new Vektor(),
      1.0, 0.);
  
  private final Vektor dummyVektor = Vektor.getDummyInstance();
  
  /**
   * <code>MAX_PARTICLE_DIFF</code>
   */
  private static final double MAX_PARTICLE_DIFF    = .4;

  /**
   * <code>MAX_MEASUREMENT_DIST</code>
   */
  private static final double MAX_MEASUREMENT_DIST = 60;

  /**
   * <code>MAX_U_LENGTH</code>
   */
  private static final double MAX_U_LENGTH         = 2.2;

  private int                 xts                  = 0;

  private int                 xt_1s                = 0;

  /**
   * <code>MCLFilter</code> constructor
   */
  public MCLFilter() {
    
    for(int i=0; i< xt.length; i++){
      xt[i] = Particle.getDummyParticleInstance();
    }
    
    for(int i=0; i< xt_1.length; i++){
      xt_1[i] = Particle.getDummyParticleInstance();
    }
    
    this.reset(new Vektor());
  }

  /**
   * <code>sampleParticle</code>
   * 
   * @param position
   *          the measured position
   * @param distance
   *          distance of the measurement
   * @return double - probability
   */
  public final double sampleParticle(final Vektor position,
      final double distance) {

    // System.out.println(position.toString());
    double p;
    if (this.xt_1s < 1) {
      p = 1.;
    }
    else {
      p = 0.;
      // resample particles t-1 with new position and sum
      for (int i = 0; i < this.xt_1s; i++) {
        p += this.sampleParticle(position, this.xt_1[i]);
      }
    }
    // probability of the measurement
    p *= this.p_y_x(distance);
    // add new particle
    if (this.xts >= this.xt.length) {
      // particle array full, replacing the oldest particle
      this.xt[0] = new Particle(position, p, distance);
    }
    else {
      this.xt[this.xts++] = new Particle(position, p, distance);
    }
    return p;
  }

  /**
   * <code>sampleParticle</code>
   * 
   * @param position
   *          the measured position at time t
   * @param particle
   *          a particle at time t-1
   * @return double - probability
   */
  private final double sampleParticle(final Vektor position,
      final Particle particle) {

    // sum component
    return this.getWeight(particle) * this.p(position, particle, this.ut) * particle
        .getProbability();
  }

  /**
   * <code>setU</code> sets a new odemetry data u, i.e. the new movement
   * vector. This also starts a new time cycle.
   * 
   * @param u
   *          odometry measurement
   */
  public final void setU(final Vektor u) {

    if (u == null) {
      return;
    }
    double max = 0.;
    double temp;
    for (int i = 0; i < this.xts; i++) {
      temp = this.xt[i].getProbability();
      if (temp > max) {
        max = temp;
      }
    }
    for (int i = 0; i < this.xts; i++) {
      this.xt[i].scale(max);
    }
    this.ut_1.copy(this.ut);
    this.ut.setProbability(this.p_y_u(u));
    this.ut.setDistance(0);
    for(int i=0; i<this.xts;i++){
      this.xt_1[i].copy(this.xt[i]);
    }
    this.xt_1s = this.xts;
    this.xts = 0;
  }

  /**
   * <code>p</code> motion model
   * 
   * @param x_t
   *          particle at time t
   * @param x_t_1
   *          particle at time t-1
   * @param u_t
   *          action at time t-1 (before t)
   * @return double - probability
   */
  private final double p(final Vektor x_t,
      final Vektor x_t_1,
      final Vektor u_t) {
    this.dummyVektor.copy(x_t_1).addToThis(u_t).subFromThis(x_t);
    return this.p_dist(this.dummyVektor.getLength(),
        MCLFilter.MAX_PARTICLE_DIFF);
  }

  /**
   * <code>getWeight</code>
   * 
   * @param particle
   *          particle to weight
   * @return double - weight
   */
  private final double getWeight(final Particle particle) {

    return this.p_y_x(particle.getDistance()) / this.p_y_u(this.ut_1);
  }

  /**
   * <code>p_y_u</code> returns the probability of u, .5 is minimum.
   * 
   * @param u
   *          u vector
   * @return double - likely value of u
   */
  private final double p_y_u(final Vektor u) {

    return (this.p_dist(u.getLength(), MCLFilter.MAX_U_LENGTH));
  }

  /**
   * <code>p_y_x</code>
   * 
   * @param distance
   * @return double
   */
  private final double p_y_x(final double distance) {

    return this.p_dist(distance, MCLFilter.MAX_MEASUREMENT_DIST);
  }

  /**
   * <code>p_dist</code>
   * 
   * @param distance
   * @param maximum
   * @return double
   */
  private final double p_dist(double distance,
      final double maximum) {

    if (Math.abs(distance) >= maximum) {
      return .00000001;
    }
    // norm
    distance /= maximum;

    return 1 - Math.pow(distance, 2);
  }

  /**
   * <code>reset</code>
   * 
   * @param position
   */
  public final void reset(final Vektor position) {
    this.ut.reset();
    this.ut_1.reset();
    this.xt[0].copy(this.dummyParticle);
    this.xt[0].copy(position);
    this.xt_1[0].copy(this.dummyParticle);
    this.xt_1[0].copy(position);
    this.xt_1s = this.xts = 1;
  }

  /**
   * <code>getPosition</code>
   * 
   * @return Vektor
   */
  public Vektor getPosition() {

    if (this.xts < 1) {
      return null;
    }
    Particle particle = this.xt[0];
    double belief = particle.getProbability();
    final Vektor v = particle.mult(belief);
    double sum = belief;
    for (int i = 1; i < this.xts; i++) {
      particle = this.xt[i];
      belief = Math.sqrt(particle.getProbability());
      // belief *= belief;
      sum += belief;
      v.addToThis(particle.mult(belief));
    }
    return (sum <= 0) ? v : v.div(sum);
  }
}
